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ABSTRACT 


The Flexible Spacecraft Simulator (FSS) at the Naval Postgraduate School 
was modified by replacing the flexible appendage with a two link robotic 
manipulator. This experimental setup was designed to simulate motion of a 
spacecraft about the pitch axis. The spacecraft consists of a main body, a two 
link manipulator, and momentum wheel actuator to control the pitch attitude of 
the spacecraft. Position information from the main body and manipulators was 
obtained from Rotary Variable Displacement Transducers (RVDT). The equations 
of motion were developed assuming that the main body and manipulator were 
rigid bodies. The resulting coupled, nonlinear, time invariant equations of motion 
were used to analyze the dynamics and kinematics of the main body and 
manipulator as well as the interaction between the main body and manipulator. 

Three different control strategies were developed using Lyapunov's Second 
or Direct Method. With the first controller, simple linear feedback of position and 
velocity information with constant gains was used to position the manipulator 
and stabilize the main body. A fifth order polynomial was used to generate a 
reference trajectory for the second controller. This trajectory was used in 
conjunction with a tracking controller to position and stabilize the system. In the 
third controller, a near-minimum-time technique was used to generate a reference 
trajectory. This reference trajectory was employed using a tracking controller 


similar to that used in the polynomial reference controller. 
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I. INTRODUCTION 


During the past few years there has been a significant increase in the use of 
robotics. Applications range from performing routine tasks in manufacturing to 
deep sea and interplanetary space exploration. The interplanetary and 
extraterrestrial environment has become the focus of research for future industrial 
development and scientific exploration. With the hazards of this environment 
and cost of manned space flight, researchers will become increasingly dependent 
upon robotics for assembly, service, and repair of equipment in space as well as 
the exploration of space itself. Due to the requirements for terrestrial and space 
applications, there has been a significant increase in theoretical and experimental 
research in the areas of robotic dynamics and control. 

Space based robotic applications differ from terrestrial applications in one 
important area. For the space based applications, no support is provided to 
stabilize the manipulator. A space based manipulator, when repositioned, imparts 
moments and forces on the spacecraft. In addition, there is no friction to dissipate 
energy that is added to the system. To counteract these forces and moments, an 
attitude control system normally consisting of thrusters in combination with a 
momentum wheel is used to stabilize the spacecraft. Control system problems are 
exasperated as the mass of the load that is being positioned becomes larger in 
relation to the mass of the spacecraft. It is this interaction between spacecraft and 
the motion of a manipulator that warrants further research. 

The primary objective for any control system is to remain stable over a wide 
range of operating conditions while still providing adequate levels of 


performance. It is desired to meet this objective in the face of hardware 


characteristics, changing loads as well as unmodeled disturbances and system 
dynamics. These requirements and restrictions present the control system design 
engineer with significant challenges. 

Before the system can be analyzed, the equations of motions must be 
determined. The equations of motion for a robotic system can easily be developed 
through Lagrange's equations and are in the form of a set of second order 
differential equations. These equations are coupled and nonlinear with 
trigonometric and higher order terms. Attempts to simplify these equations result 
in equations of motion that are valid over a limited range of motion or for specific 
boundary conditions. The current trend in trajectory control requires highly 
nonlinear maneuvers that are valid over a wide range of applications and 
operating conditions. These requirements dictate that the full, nonlinear 
equations be used to describe the motion of the system. With the introduction of 
the nonlinear equations of motion, many traditional tools in control theory used 
to analyze linear, time-invariant systems are not available or are meaningless. 

Recently, research by Junkins [Ref. 1] and Bang [Ref. 2] has revived interest 
in using Lyapunov's second method for a flexible structure control system design. 
This technique is very attractive because it can be applied to nonlinear, time 
invariant, systems with guaranteed stability for a wide range of conditions. An 
important feature of Lyapunov's second method is the freedom to select the 
Lyapunov function and the corresponding feedback control law. The Lyapunov 
function can be selected based on physical insight and the control law can be 
selected to ensure that the system is stable. The Lyapunov function must be 
positive definite and is normally related to the system energy for a large class of 


mechanical natural systems. The control law can be selected such that the 


Lyapunov function or system energy will always decrease to zero or some 
equilibrium point. 

The purpose of this thesis is to apply a general methodology for finding 
Lyapunov stable control laws for stabilizing the spacecraft main body while 
controlling a two link manipulator attached to that spacecraft. A complete 
description of the experimental setup is discussed in Chapter II. Topics include 
the physical characteristics of the manipulator, main body, actuators, sensors, test, 
simulation, and data collection equipment. In Chapter III, the coordinate systems 
and the equations of motion are developed. Three different control strategies 
were developed using Lyapunov's Second or Direct Method. With the first 
controller, simple linear feedback of position and velocity information with 
constant gains was used to position the manipulator while stabilizing the main 
body. A fifth order polynomial was used to generate a reference trajectory for the 
second controller. This trajectory was used in conjunction with a tracking 
controller to position and stabilize the system. In the third controller, a near- 
minimum-time technique was used to generate a reference trajectory. This 
reference trajectory was employed with a tracking controller similar to that used 
in the polynomial reference controller. Simulation results are presented in 
Chapter IV. Chapter V includes a summary of the conclusions as well as topics 


for future research. 


П. EXPERIMENTAL SETUP 


A. SPACE ROBOTICS SIMULATOR DESCRIPTION 

The Spacecraft Robotics Simulator (SRS) is a modification to the Flexible 
Spacecraft Simulator (FSS) used for previous work by Hailey [Ref. 3], and 
Watkins [Ref. 4]. The FSS was modified by removing the flexible appendage 
and replacing this appendage with a two link manipulator. The SRS consists of a 
central main body with a two link robotic manipulator. Pitch axis control of the 
main body is provided by a single momentum wheel driven by an electric servo- 
motor. The central body was constrained to rotational motion only by an I-beam 
mounted over the over the granite table. The main body and manipulator were 
supported by air bearings that float upon a thin cushion of air on an optical 
quality granite surface. Each of the two links were positioned via geared DC 
servo-motors. A Rotary Variable Displacement Transducer (RVDT) was used to 
obtain position information at each joint for position feedback. This setup was 
designed to simulate a zero-gravity environment in two dimensions. The SRS is 


depicted in Figure 2.1. 


B. EXPERIMENT DESCRIPTION 


The SRS is composed of the following major components: 
° Spacecraft Main Body 


° Two Link Robotic Manipulator 
° RVDT Position Sensors 

° Granite Table 

° Electrical Power Supplies 

° AC-100 


° VAX 3100 Series Computer 





Figure 2.1 Spacecraft Robotics Simulator 


1. Spacecraft Main Body 

The spacecraft main body consists of a rigid, 7/8 inch thick, 30 inch 
diameter, aluminum disk. The main body is designed to simulate the two 
dimensional planar motion of a spacecraft about its pitch axis. The main body is 
Supported by three air bearings spaced at 120 degree intervals. Each of the 
bearings is capable of supporting a load of 60 pounds. A fourth air bearing 
Supported by an overhead I-beam constrains the spacecraft main body to 
rotational motion only. The air bearings are designed to float the spacecraft main 
body on a thin film of air supplied by an external air source. A RVDT, model 


R30D, was connected to the rotor of the air bearing by a bellow-type device. 


The RVDT was manufactured by Schaevitz Sensing Systems was used to measure 
angular displacements of the spacecraft main body. 

Attached to the spacecraft main body was a 10.7 kilogram steel 
momentum wheel and servo motor. The momentum wheel was designed to apply 
a torque to the main body by increasing or decreasing the angular velocity of the 
momentum wheel. The motion of the spacecraft about its pitch axis was 
controlled by the torque generated by this momentum wheel. The servo motor, 
model JR16M4CH/F9T used to drive the momentum wheel was manufactured by 


PMI industries. Characteristics of this motor are presented in Table 2.1. 


TABLE 2.1 Momentum Wheel Actuator Characteristics 


Characteristic | um | 
Ma — | | PMEMoton Technologies. 
Model ить || шөмаы 

| 1» GE 


Raed Vonage 1 ves | е _ 
мн 


An integral analog tachometer, model ARS-C121-1A, manufactured by Watson 





Industries, Inc. was mounted on the servo-motor to measure angular velocity. 


A more detailed description of the motor, momentum wheel, and spacecraft main 
body can be found in [Ref. 3], and [Ref. 4]. 
2. Two Link Manipulator 

Attached to the main body was a two link manipulator. Components for 
the manipulator were designed and built by the Aeronautics and Astronautics 
Department at the Naval Postgraduate School. All components for the arm were 
manufactured from 7075 and 6061 series aluminum. All components were 
connected with SAE grade 8 medium carbon chrome alloy cap screws. A picture 


of the manipulator can be found in Figure 2.2. 





Figure 2.2 Two Link Manipulator 


The lınks of the manipulator were positioned by two geared, servo-disk motors 
manufactured by PMI. A third motor, identical to the first two actuators and was 
mounted on the tip of link two for the purpose of orienting a simulated tool or 
pointing some type of antenna. Characteristics for the link actuators are 
presented in Table 2.2. At each joint, a RVDT, model R30D, manufactured by 


Schaevitz was used to measure relative angular displacement. 


TABLE 2.2 Link Actuator Characteristics 


Characteristic | um | _ 
Manufacturer | | PME Motion Technologies 
Model Number — | | во 
Rated Tongue | жағым | о 

— s | 


» 


Rated Voltage volts 12 


Height inches 3 


The power supplies used to drive the actuators of the manipulators were 





manufactured by Kepco Inc. of Flushing, New York. The Kepco series BOP 
Bipolar Power Supplies were designed to be fast, programmable, fully dissipative, 
linear amplifiers. The BOP power supply is an all solid-state design, featuring 
integrated circuit operational amplifiers in the control circuit section and silicon 


power transistors mounted on special fan-cooled heat sinks in the complementary 


power stage. Characteristics for the Kepco model BOP 20-10 power supply аге 


presented ın Table 2.3. 


TABLE 2.3 Kepco Power Supply Characteristics 








watts 


| 
losed Loop Gain 
алдыла 
| өш | бештен тов) 
| microseconds | 60 eunen mode) 
Recovery Step Load 
microseconds | 20 eurent modo) | 


The BOP can be operated in either the voltage or current mode through two 





os O I7 IZ IO 
о [оь |с 
~ ЈЕ Ë | 
= 
= {к Is [= 
а е је 12 
дэ ICO mI O 
= |Д 
о IO 
Оо |" |5: 
е-е 






bipolar control channels. These modes are manually selectable through the front 
control panel or through remote signals. Each of the principal control channels is 
protected by bipolar limit circuits. All control and limit channels are connected to 
the output stage via an "Exclusive-Or" gate so that only one channel is in control 
of the BOP output at any one time. The BOP output can be programmed over its 


full output range by a +10 volt signal applied to either one of the inputs to the 


voltage or current channel. The limit control channels can be remotely controlled 
by a0 to +10 volt signal applied to there respective inputs. 
3. Granite Table 

The entire mechanical assemblage, including the main body and 
manipulator were supported by air bearings that float on a thin cushion of air on a 
granite table with dimensions of 8 feet by 6 feet by 10.5 inches thick. The surface 
of the table was highly polished to optical quality grade A (0.001 inch peak to 
valley). The smooth surface allows the air bearings to float freely over the surface 
of the granite table to minimize the effects of friction on the motion of the main 
body and manipulator. The granite table was carefully leveled to eliminate 
gravity induced accelerations. The mass of the table provided an extremely stable 
platform upon which to conduct the experiments. 

4. AC-100 

The AC-100 is a microprocessor based, programmable, real time control 
system manufactured by Integrated Systems International, Inc. of San Jose, 
California. The AC-100 was designed to automate the development of real-time 
systems by combining graphical modeling tools with a real-time controller. In 
addition to modeling and controlling, the AC-100 was also capable of data 
collection and storage. The AC-100 consists of the following major components: 
Intel 80386 Application Processor, Intel 80386 Multibus II Input / Output 
Processor, Intel 80386 Communication Processor, Intel 80387 Coprocessor, 
Weitek 3167 Coprocessor, Analog To Digital and Digital To Analog Digital Data 
Translation DT2402 Input/ Output Board, Two - INX-04 Encoder and Digital To 
Analog Servo Boards, Ethernet Interface Module, and Cabinet Enclosure and 


Power Supply 
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The software tools used with the AC-100 include a Design Package and 
a Run-Time Package. The Design Package included Matrixx, System Build, and 
Auto Code. These tools are used for analysis, design, and code generation 
respectively. The Run-Time Software Package provides the Graphical User 
Interface (GUI) cross compiler, device drivers, data acquisition, and Ethernet 
interface required to run software code generated by Auto Code on the AC-100. 

5. VAX 3100 Series Computer 

The VAX 3100 Series Model 30 workstation was configured with 8 
megabytes of main memory, a 19 inch (diagonal) color monitor, two 104 megabyte 
Winchester hard disks and a mouse. The VAX workstation is capable of 2.8 
Million Instructions Per Second (MIPS). 


C. SYSTEM INTEGRATION 

The AC-100 is integrated with a VAX 3100 Series computer via the Ethernet 
interface. In this system, the VAX 3100 computer was used to analyze and design 
the control system. Auto Code was then used to convert the final control system 
design into fully compiled and linked "C" computer code. This code was then 
down linked to the AC-100 via the Ethernet interface. The control system, in the 
form of C - code software can then be used to control the servo-amplifiers which 
in turn were used to drive the actuators of the manipulator in real time. Manual 
control of the system is still provided through the VAX computer acting through 
the Ethernet interface. A block diagram of the integration of the control system 
for the two link manipulator and spacecraft main body are presented in Figure 


2.3. Mass and inertia characteristics are summarized in Table 2.4 
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Figure 2.3 Control System Integration 
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TABLE 2.4 Mass And Inertia Characteristics 


ami | ms | ke | 2%% _ 
| ima) | em | oo _ 
| пето | im? | оо _ 
— и 
am | ms | e | oç _ 
| he | km? | 0352 _ 
s шай | ош _ 
сомы] = заво 0 
мат Воду | Ме | k | s _ 
И геа 
и 
| пей | em | oo _ 
Loo | CmerorMas | m | 2 


Notes: (1) Moment Of Inertia About Arm Axis Of Rotation. 
(2) Moment Of Inertia About Center Of Mass. 
(3) Center Of Mass Location With Respect To Axis Of Rotation. 
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ПІ. ANALYTICAL MODEL 


The Spacecraft Robotics Simulator (SRS) consists of a central main body to 
which is attached a two link robotic manipulator. The motion of the main body is 
constrained to move in a plane resulting in a system with three degrees of 
freedom. This planar motion constraint greatly simplifies the problems associated 
with the derivation of the equation of motion and control system design yet still 
retains the most critical analytical elements. The manipulator and main body were 
modeled as rigid structures. Lagrange's equation was used to derive the 
equations of motion. This section will first describe how the equations of motion 


were derived and then how the control system was developed. 


A. COORDINATE SYSTEMS 

Before the spacecraft attitude and manipulator can be controlled the 
dynamics of the system must be carefully defined and understood. The first step 
in this process, is to determine the equations of motion for the system. 

In this model, the main body is constrained to rotational motion only. Four 
different coordinate systems were used for this analysis. The №, №, N3 axis for 
this problem was fixed in inertial space coincident with the axis of rotation of the 
main body. The coordinate system, XL yı, 71, is fixed in the main body with the 
X] axis pointing toward the attachment point for link 1. The хі, yi, 71 coordinate 
system ıs obtained by rotating about the N3 axis of the inertial coordinate, by an 
angle of 0). In a similar way, the coordinate systems, X2, y2, 72, is fixed at the 
axis of rotation of link 1 and points toward the attachment point for link 2. The 


— 


X5, уз, 72 coordinate system can be obtain by rotating about the Мз axıs of the 
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inertial coordinate system by an angle of 82. The coordinate system, X3, уз, Z3, is 
fixed in the body of link 2 with the X3 axis pointing toward the end point of link 
2. This coordinate system is obtained by rotating by an angle, 63 about the N3 


axis Of the inertial system. Coordinate systems are presented in Figure 3.1. 
О Servo Motor 


C— Manipulator Arm 


Local Coordinate Axis 


Inertial Axis 


Momentum 
Wheel 


Satellite Main Body 





Figure 3.1 Inertial And Local Coordinate Systems 
АП апрјез, 61, 02, апа 03, as well as vectors rj, I^, and r3 are defined in terms 


of inertial coordinates where these quantities are defined in Figure 3.1 
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= Center Of Mass 


О Servo Motor 


Manipulator Arm 


Inertial Axis 


Satellite Main Body 





Figure 3.2 Inertial Angles And Vectors 


The vectors 1, 7, and r3 describe the location of the center of mass of each of 
the manipulators in terms of inertial coordinates. The angles 021, 031, and 032 
represent relative displacements of the joints and can be derived from the inertial 
coordinates 01, 65, and 03 in such a way that 021 = 82 - 01, 031 = 03 - 01, and 
032 = Ө; - Ө». 
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В. EQUATIONS OF MOTION 

A momentum wheel was used to apply a control torque (о ће тат Бойу. 
The motion of the momentum wheel is de-coupled from the motion of the main 
body. It is assumed that the momentum wheel is only an actuator that is used to 
control the motion of the main body and counteract torques generated by the 
movement of the manipulator. 

1. Lagrange's Equations Of Motion 


Lagrange's equation for n-dimensional dynamic systems are stated as 


d a .9L - OF wher п (9) 
dt\dqi/ 94; 
L=T-V 


V - Potential Energy 

T - Kinetic Energy 

qi- i" Generalized Coordinate 
qi- i Generalized Velocity 


Q;- i? Applied Non conservative Force 
For this particular problem, the system requires three degrees of freedom 
to describe its governing equations of motion. The generalized coordinates and 


velocities are chosen to be in terms of inertial coordinates as presented below. 
T 
4-(01, 62,063)  -|41, Ф, аз)! (3.2) 


37 ATO... M 
q- |д\, 0, 03| z (di. 92,93)" (3.3) 
2. Kinetic Energy 
The first step is to determine the total kinetic energy of the system. The 


total kinetic energy of the system is given by 


Im 


3 
1 T; (3.4) 


22 > 
T; = 5 I; Ө; +7 m; {г A (3.5) 
I; - i” Mass Moment Of Inertia About Center Of Mass 
m; - i? Mass 
AM оз 
po > > lH 1/0; 6; 5 (3.6) 


1-11-1 


A more detailed algebraic procedure can be found in Appendix B. 


3. Equation Of Motion 


Lagrange's equations can be applied to equation (3.4) and the terms can 


be arranged in matrix form so that the final form can be written as (3.9). 


where 


and 





M(6) 6 clo, 6) 2 Q (3.9) 
П111 1112 П113 
М(9)- шш m22 па 
11131 11132 11133 (3.10) 
mj; 2 Ij + L; [mi + m + mj + пы 14 (3.11) 
2 Be 
np? 2 lcm; L2 m (Lem: + m (3.12) 
2 
таз = "Пена Пени (3.13) 
mp =m] = [4 15 со8(021| m [из + па | (3.14) 
2 
my3 =M31 = Ly Lem, cos(@31) m3 (3.15) 
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0123 = 1132 = L2 Leg, cos(032) ms (ОЛО) 
G(1) 


G(6,6) =| G(2) (3.17) 
G(3) 


G(1) =| (m2 Li E + (m3 L4 L2) | sin (021) > + 


| | (3.18) 
(тз 1л 1с.) 5їп (Өз) 03 
G(2) =-[ (m2 Ly т 15) ] віп (951) 01 + ө 
(m3 Lz Lem.,) sin (631) Өз | 
G(3) = - (m3 Ly Lem.) sin (82) 61 - athe 


(m3 L2 Lem.,) sin (839) 0: 

4. Applied Torques 
D' Alembert's principle for virtual work expression was used to determine 
the expressions for Q for the equations of motion. As written, Q is a vector 
containing the torques applied by the actuators at each joint in terms of inertial 
coordinates. At this point it will be beneficial to rewrite the Q vector in terms of 
local coordinates. The virtual work applied to the system is given by the 


following equation. 


3 3 
8W =), 0;80;=), 8W; (3.21) 


1=] 121 
The elements of Q are in terms of torques applied to inertial coordinates. Since 


each actuator applies a local torque, it was beneficial to rearrange (3.21) and write 


Q in terms of the local torques applied by the individual actuators. The virtual 


work described ın equation (3.21) can be described in terms of these local torques 


as follows. 
OW , Zu, 004 - u5 004 (3.22) 
OW,= 12 00 - из 00; (3.23) 
О\!з = из 695 (3.24) 


In matrix format Q can be written in terms of a control influence matrix B and the 


local torque vector, u as written in (3.25). 


О=Ви=[О1, О», Оз]! (3.25) 
Ој = ш - 92 (3.26) 
Q2 = u2 - u3 (3.27) 
Q3 = u3 (3.28) 
where 
1-10 ul 
B= 0 1-1 Ч= 02 
001 из 











C. LYAPUNOV STABLE CONTROLLER DESIGN 

This design for a stable non-linear controller is based upon Lyapunov's 
Stability Theory. This theory is also known as Lyapunov's Second or Direct 
method. This theory is covered in greater detail in [Refs. 1, 2, 14, and 15]. To 
review the Lyapunov Stability Theory, a system without any external forces or 
torques is assumed. This system is assumed to have a single equilibrium state. For 


this system, a positive definite function 1s assumed to be an exact integral of the 
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system under some idealization. This function is termed a Lyapunov function and 
is selected to satisfy the requirements that it is zero at the desired equilibrium and 
positive everywhere else. This function may be represented by the system total 
energy or the Hamiltonian of the system for most of the time invariant mechanical 
systems. If this system is perturbed from its equilibrium state, the energy state is 
increased to some positive energy level. Depending upon the nature of the 


Lyapunov function, one of the following conclusions can be drawn. 


e If the system dynamics dictate that the initial energy of the system does 
not increase with time, then the system is stable. 


° Ifthe energy of the system monotonically decreases with time for all initial 
conditions, and eventually goes to zero, then the system is asymptotically 
stable. 


° Ifthe energy increases for any initial condition, then the system is unstable. 


• If the energy measure neither increases, nor decreases as a function of time 
then no conclusion can be drawn. 


Despite the power of this theory, there is no unified process to find candidate 
Lyapunov functions that globally satisfy stability requirements. A more complete 
description of the Lyapunov stable controller design for flexible structures can be 
found in [Refs. 1 and 2]. 

1. Lyapunov Stable Controller With Linear Feedback 

For this application, a simple non-linear controller with linear feedback of 
position and velocity information is the primary objective. The candidate 


Lyapunov function for this application is of the form 


U=E+ f (58), 00, 603) (3.29) 
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where E is defined as the “work energy’ of the Lyapunov function and "f" is a 
positive function of 601, 602, and 004. Note that "f" is a pseudo potential energy 
that renders the Lyapunov function positive definite with respect to the new 
equilibrium point. In addition, 60; — 0; - 0j, where 0; is a constant that describe 


the final joint angle. 


(01, 05, 05, 61, 05, 03) f = (011, 027, 07, O, O, O) (3.30) 


The Lyapunov function in (3.30), can be differentiated to obtain 


ШЕ Е-> of 0, (3.31) 


450) 


The "work energy мн = of (3.31) can be directly obtained from (3.22) through 
(3.24) and the "pseudo energy rate" of the system is defined in (3.33). 


E = (u; - 12) 50, + (02 - из) 50, ua 503 (3.32) 
~ f 
"Pseudo Energy Rate" = 2. 2056) 356) 50; (3.33) 


Equations (3.32) and (3.33) can be substituted into (3.31) to form equation (3.34) 


which can be further simplified to equation (3.35). 


U = (11 - 02) 50, + (02 - из) $0, ШЗ 503 








з, of (3.34) 
+S oe 
2 460) 
U = 80, (u; - uo + 3s * e үт -— 
501) 386.) (3.35) 
+ 003 [us + "m 
4505) 


Based on (3.35), it is evident that a function can be selected such that U< 0, 
which is the stability condition in the Lyapunov sense. Therefore, the control 


laws are chosen to satisfy the following relations 
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of 





Ui - U2 + - -g,, 60 326 
=й? У 01, 091 ( ) 
12-14 50 2 37 
Еа 365) P = -22, 092 ( ) 
= -23, 603 (3.38) 
“ра” 
where, 
ВИНО 202212 22 0 апа ра 20 
for 
f > 0 
In addition, 
U = - | 20 Som нас 56, mE. 864 | (3.40) 


By selecting an appropriate function for "f" equation, the stabilizing control laws 
(uy, U2, U3) satisfying ће О < O requirement can be built. For this case, assume 


that "f" 1s defined as follows 


Їнэ (eh, 504 + BO 605 T Ë 3. 802) 


(3.41) 
where 

о О О 
For a controller using pure linear feedback, (3.36-3.38) can be simplified as 


described below. 


из =- 23, 603 - рз, 803 (3.42) 
92 =И3- 52, 082 - Zo, 60, (3.43) 
u =u2 - gi, 66; - g1, 601 (3.44) 
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од; = 6, - О о 25 (3.45) 


50; = Ө; - Ө, (3.46) 


where 


8, = Ufer 103 
2. Lyapunov Stable Tracking Controller 

In a similar way, the Lyapunov stable tracking controller can also be 
derived. Lyapunov stability is not proven here but is discussed in more detail in 
[Ref. 1]. In this application, a function generator was used to generate a reference 
trajectory for the controller to follow. With this type of controller, a fifth order 
polynomial was used to generate a desired tip trajectory with a two link closed 
loop inverse kinematic solution in one case. In another case a "near-minimum- 
time” torque shaping scheme was utilized to generate a reference trajectory. Both 
these trajectory generators will be discussed in more detail in following sections. 


The control torques required for this reference tracking controller are presented 


next. 
биз = - 25, 603 - 23, 603 (3.47) 
би? - биз - 82, 002- 82, 00) (3.48) 
би] = би? - 21, 001 - 21, 50, (3.49) 
where 
80; = Oi - Oia (3.50) 
50,)-0:-0,, (3.51) 
Ou; = Uj - Ui (3.52) 
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The control laws presented above can be rigorously proven to satisfy the 


Lyapunov Stability condition if 60; and 50, are very small. 


D. REFERENCE TRAJECTORY GENERATION 

For the tracking controller developed previously while discussing the 
Lyapunov stable tracking controller requires some reference trajectories. Here 
trajectory refers to a time history of position, velocity, and acceleration for each 
degree of freedom. The primary consideration for generating a trajectory lies in 
the fact that the trajectory must first of all be smooth and second it must be easily 
calculated. In this thesis, two different techniques will be used to generate the 
required trajectory information to stabilize and control the system. 

The first trajectory that will be discussed involves using a fifth order 
polynomial to describe the path of the tip of the link three of the manipulator. In 
the second trajectory, a near-minimum-time maneuver using input torque shaping 
will be used to generate the desired trajectory. 

1. Polynomial Reference Trajectory 

For the tracking controller discussed in the previous section, it is usually 
difficult if not impossible to obtain the open-loop solutions for the theoretical 
reference system of differential equations. For practical considerations it is often 
advantageous to design the control system that will follow an easy to calculate 
path. For robotic applications, a polynomial of order 3 or higher is often used to 
specify the position of the end of the manipulator. For this application, imagine a 
two link manipulator attached to the spacecraft main body as depicted in Figure 
3.3. In this case, the maneuver attempts to position link two from an initial angle 
of 20 degrees to a final position of 40 degrees and link three is maneuvered from 


40 to 60 degrees. For a two link manipulator, there exists a closed loop solution 
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to the inverse kinematic problem that can be quickly and easily computed. The 
closed loop solution is described in more detail in appendix B. For this 
manipulator, given the beginning and final coordinates of the manipulator, the 
vector r is used to specify the position of the end point of link two as a function 


oftime. This vector is presented in Figure 3.3. 


” Reference Maneuver 
Toe Polynomial 


Momentum 
Wheel 


p 
> 





Figure 3.3 Polynomial Reference Maneuver 
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2 = F(t) + (9 [Е (9) - zu] (3.53) 
To simplify the calculations, t can be written as a function of normalized time, Т, 
given the start time tg and finish time ts. Note that for to <t <t,, thenO<T< 1. 
т= lo (3.54) 
tf- 0 
A fifth order polynomial was used for this application. This allowed the 
user to specify the beginning and final velocities and accelerations as well as the 
beginning and final positions of the tip. The polynomial for this controller was of 


the form 


f(T) =C] + C2 T+ C3 TŽ + c4 T? + Cs T + Cç (3.55) 


and was subject to the following boundary conditions. 
f (0) = 0,f (0) = 0,f (0) = 0 


f (1)=1,f(1)=0,f(1)=0 
The resulting expression for f(T) is obtained as 
Кт) =10 13 - 15 13 + 6 12 (3.56) 
Equation (3.56) can be differentiated with respect to time. These 
expressions were utilized to calculate velocities and accelerations and are 


presented in equations (3.57) and (3.58). 


f(t) = 30 72 - 60 13 + 30 7 (3.57) 


°° 


т) = 60 т - 180 2 + 120 7° (3.58) 
Equation (3.53) can also be differential with respect to time to give the velocity 


and acceleration of the tip of link three purely as a function of time. 


Vector Position: 
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г (т) = тоо) + ї(т)[г(«)-т(ю/| ` (3.59) 


Vector Velocity: 


r (1) =f (1) pomo (3.60) 
Vector Acceleration: 
г (т) = f (1) — (3.61) 
2 


With the position of the tip known as a function of time, the angles 02 
and 932 can be solved for directly as can 05 and 05 via the two link closed loop 
solution of the inverse kinematics problem. At this point 91, 01, and 01 are all 
assumed to be comparatively small. The Jacobian can be used to define the 
relationship between the velocity and angular velocity as well as the tip 


acceleration and angular acceleration as described in the following equations. 


T =H fÔ! (3.62) 


where H is the Jacobian corresponding to the given configuration. 





f -ksin (@2), -1 sin (65) (3.63) 
15 соѕ (0), _ 15 со (03) | 
r =H (6}+H (0) (3.64) 
_| -hcos СЯ! - ІЗ со5 (Өз) | (3.65) 
- 5 sin (Өз), - 13 sin (Өз) | 





With the above equations, the position (angular and cartesian), velocity 
(angular and cartesian), and acceleration (angular and cartesian), can all be 


determined as functions of time. With this information, and the physical 
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characteristics of the system, the reference torques can also be determined using 


equation presented below. 


uer = B` [M (0) ө + С(0,0) (3.66) 
2. Minimum-Time-Maneuver 

For an ideal case, of a single degree of freedom system, the minimum time 
required to perform a particular maneuver is achieved by applying the maximum 
available torque for one-half of the time required to complete the maneuver 
followed by the remaining half with the maximum negative torque. This results in 
a controller where the torque is always operated at its maximum value and gives 
rise to a torque shaping function, position, velocity, and accelerations profiles as 
presented in Figure 3.4. This type of controller is sometimes referred to as a bang- 


bang controller. 
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Figure 3.4 Bang-Bang Minimum Time References 
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For an application with a single degree of freedom, the relationship 
between the angular acceleration and the applied torque is given by equation 


(3.67) with the applied torques given by equation (3.68). 
10-1 (3.67) 


Ше for O<ı<] 


ЙГ її. о Р <t<tr (3.68) 
| O, fortc €t | 
Bang-Bang control theory for minimum time maneuvers is discussed in more 
detail in [Refs. 1 and 2]. 
3. Near-Minimum-Time Rigid Body Maneuver 

A more general case of the bang-bang, minimum time controller is the 
near-minimum-time controller. The bang-bang controller does maneuver the 
manipulator from the initial to final positions in the minimum amount of time. The 
primary drawback of the bang-bang controller is the rapid rise of the torque 
trajectory. This results in a rapid acceleration followed by a rapid deceleration 
which will require actuators with instantaneous switching capacity which is not 
practical and a robust structural design for the manipulators themselves. By 
introducing an input shape function, the instantaneous rise in the torque 
trajectory can be reduced, resulting in slightly smaller accelerations which will in 
turn require smaller actuators and reduced structural requirements. For 
comaparison, the input shaping function for & = 0.1 and ü = 0.25 are presented 
ın Figures 3.5 and 3.6 respectively. 

The differential equations used to describe the minimum time maneuver 
above can be modified so that they are of the form. 


I Өгег = Uref = Umax f(At,t,,t) (3.70) 
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Figure 3.6 Normalized Input Shaping Funciton With a = 0.25 
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Where the input shaping function is suggested in the form. 


= [| 3- 2 | L| foro ste At 


= 1 for Atst<tj 


Ае er (uf Е 22 |а 


2At 2At 
=- 1 107 2 << 5 


| ај |. Ыз 


| for (1 <1< (5 








] fort stet 








(3.69) 


where 
te = Maneuver Time 
t =t -At 
tp =t+At 
t3 = tr - At 


At=Qt; =Rise Time 
ts = B tr = Switching Time, where B = 0.5 
To determine a relationship between the angular acceleration and the 


maximum available torque we begin with the non-linear equations of motion 


мје) 6 + аје, 6) = B u (3.70) 


where 
М(0) - Mass Matrix 
cle, Ө) - Coriolis Acceleration Terms 


u - Local Torques 
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Equation (3.70) ıs linearized by eliminating all non-linear cosine, sine, and higher 
order terms. These linearization process results in the following equation for an 


i? link. 
I Oef = Uref = Umax F(At, tet) (371) 


I - Moment Of Inertia About Axis Of Rotation 
Equation (3.71) can be reorganized into the form of the following equation. 


Over = аш НАМЫ) (3.72) 


In this equation, Umax is a design parameter that is determined by the 
characteristics of the actuator used to drive the manipulators. For this application, 
all the actuators are the same geared motor. f(At,t,,t) can be modified by varying 
the a variable. I, the linearized mass moment of inertia, is determined by the mass 
characteristics of the system and does not vary with time or changing geometry. 
Given a maximum torque Umax, and an input shaping function with suitable 
boundary conditions, the reference position and velocities can be determined by 
integrating the following equations piece wise of the time interval determined by 
the maximum torque. 


reelt) = 00 4 E | f(At,tr,t) dt (3.73) 
to 


t 
TI 


деко = Oo + Ө, (t-to) + e | f(At,tr, t9) dt? dti (3.74) 


to 
to 
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where 
to = 0 6(0) = бо (0) = 0 
t- ty Ө (0) = Ө; &(t) 2 0 
are the boundary conditions at the start and completion of the maneuver. The 
resulting reference equations are presented in Appendix B. 
The near-minimum-time to complete the maneuver can be obtained by 


setting t — tr in the equation presented below and obtained from Appendix B. 


l la+- got? 
alarm Ë 2 10 4 x 


By doing this, a relationship between the time to complete the maneuver 





and the maximum torque available can be determined for an i link. 


(3.76) 





(3.77) 


“4 14-44 


For this application, the time derived with the above equations represents the 


near-minimum-time required to perform the maneuver for only one joint. 


E. ANALYTICAL MODEL SUMMARY 

This research project analyzes how a two link manipulator can be controlled 
from a spacecraft main body while minimizing the effect of manipulator motion 
upon the main body In general one can calculate the interaction force between 
the links and main body by using the Newtonian approach with free body 
diagrams. Based on this analysis, the more smoothly that the maneuver is 
performed, the smaller the interactive force will be. Two basic type of controllers 


were developed. In the first controller, linear feedback of position and velocity 
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information from the joints was used to control the endpoint position. In the 
second type of controller, position and velocity information are used in 
conjunction with a reference trajectory to control endpoint position. 

For this type of controller, two different schemes were used to generate the 
reference trajectory. In the first, a fifth order polynomial ın conjunction with the 
two link closed loop solution for the manipulator inverse kinematics and the 
Jacobian were used to generate the reference trajectory. In the second, a near- 
minimum-time torque shaping technique was used to determine the reference 
trajectory. All components, including the manipulator and main body, are 


modeled as rigid bodies. 
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IV. RESULTS AND DISCUSSION 


Analytical results from the simulation are presented in this section. Three 
different techniques were used to control and stabilize the position of the 
manipulator. Results from the linear feedback controller with constant gains, 
polynomial reference tracking controller and near-minimum-time reference 


tracking controllers are presented sequentially. 


A. REFERENCE MANEUVER 

The three most import criteria with respect to this research are the end tip final 
position, joint torques, and the effect of manipulator motion on the attitude of the 
main body. Final position for the endpoint of the manipulator is critical for 
performing assembly tasks. Joint torques are important to verify if the servo- 
actuators can perform the desired maneuvers. The effect of the manipulator 
motion upon the main body is critical due to the excitation of flexible structures 
on the spacecraft or the effect upon antenna pointing accuracy to maintain a 
communications link. 

To be able to effectively compare the performance of the three different 
controllers, a standard reference maneuver was developed. For this maneuver, the 
desired rotation of the main body was zero. Link 1 was programmed to move 
from an inertial angle of 20 degrees to 40 degrees. Link 2 was programmed to 
move from an initial inertial angle of 40 degrees to a final position of 60 degrees. 


This reference maneuver is depicted in Figure 4.1 
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Figure 4.1 Reference Maneuver 


B. LINEAR CONSTANT GAIN FEEDBACK CONTROLLER 

The theory behind the linear constant gain feedback, controller is described 
in more detail in Chapter III and [Ref. 1,15]. Position gains of 1 ( Gp = 1 )and 
velocity gains of 5 (Gy =5 ) were used for these plots. Small gain ( Gp = 0.1 and 
Gy = 0.2 ) plots are presented in Appendix D for comparison. 

The linear feedback controller was the simplest controller conceptually and 


the easiest to implement. This controller provided stable control over a wide 
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range of gains and for a wide variety of maneuvers. No knowledge of the system 
mass or inertia characteristics was required for this controller. Only the position 
and velocity gains, and beginning and final arm positions were required to 
calculate the required control torques to perform the maneuver. 

The joint time history for this case is presented in Figure 4.2 and the joint 
velocity time history ıs presented ın Figure 4.3. Performance of this control 
system was directly related to gain selection. The system was relatively sensitive 
to small changes in the position and velocity gains. This resulted in changes to 
the maneuver time and control system damping. The system was stable for all 
gains and maneuvers evaluated and it could be proven by Lyapunov stability 
theory that the system was globally stable. Even with system stability not really 
in question, there was no way to systematically select position and velocity gains 
to optimize maneuver time or torques to achieve desired performance measures. 

The magnitude of the gains directly impacted the performance of the 
manipulator and the stability of the main body. Larger gains generated larger 
control torques and reduced the time required to perform the maneuver. The 
damping of the system could also be varied with the gain selection. The effect of 
this movement on the main body was small but still resulted in a displacement of 
nearly 2 degrees. Although this is apparently a small displacement, 2 degrees may 
still cause degradation in communication with the satellite due to antenna 
pointing errors. 

Time history plots for torques and momentum wheel speed are presented in 
Figures 4.4 and 4.5 respectively. The response of this system to the controller 


was characterized by a very rapid increase in torque immediately after initiation of 
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Figure 4.2 Linear Constant Gain Feedback Joint Position Time History With 
Gp=1 and Gy=5 
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Figure 4.3 Linear Constant Gain Feedback Joint Velocity Time History With 
Gp=1 and Gy=5 
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Figure 4.4 Linear Constant Gain Feedback Torque Time History With Gp=1 
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Figure 4.5 Linear Constant Gain Feedback Momentum Wheel Speed Time 
History With Gp=1 and Gy=5 
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the maneuver followed by a rapid decrease after a few seconds. This response 
was similar to an impulse input that became more pronounced with increased 
commanded motion or with increased gains. Large gains are desirable to achieve 
acceptable levels of performance but must be balanced against actuator 
characteristics , magnitude of the maneuver, and structural considerations.. A 
large and sudden increase in commanded torque could saturate the actuator or 
excite flexible appendages attached to the spacecraft. 

The large initial torques, depicted in Figures 4.4 and 4.5, correspond to 
rapid changes in the speed of the momentum wheel. Large speed changes are 
required during the initial portion of the maneuver to generate large toques to 
counteract the forces generated by the manipulator. Based on previous 
experience with the FSS and analysis of electrical power requirements of the 
actuator and power supply, it 1s reasonable that the actuator will be able to 


compensate for the torques generated by the movement of the manipulator. 


C. POLYNOMIAL REFERENCE TRACKING CONTROLLER 

To rectify the problems encountered with the simple linear constant gain 
feedback controller a reference tracking controller was implemented. In the 
previous case, there was a large initial error for which the system was attempting 
to correct. In this controller, a smooth, fifth order polynomial was used to 
generate a reference trajectory so that the correction could be evenly partitioned 
over a specified maneuver time. With this information, reference torques, angular 
position, velocity, and acceleration were obtained as a smooth function of time. 
As a result the initial "jump" generated by the linear feedback controller was 


eliminated and the manipulator was able to track a smooth path from initial to 
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final conditions. A variation of the Lyapunov stable control law used in the 
previous controller was used with this reference trajectory controller. 

With this controller, the time required to perform the maneuver could be 
specified but was subject to the characteristics of the actuators and the task being 
performed. Two maneuver times were analyzed. In the first case, the time to 
perform the maneuver was fixed at 2.5 seconds. This time was selected to 
determine joint torques, velocities, and momentum wheel speeds while operating 
near the torque limit of the actuators. In the second case, a time of 5 seconds 
used to determine the effect of maneuver time on the actuators. These plots for 
the 5 second maneuver time are included in Appendix D to demonstrate the effect 
of maneuver time upon joint torques and velocities. 

Position and velocity time history of the manipulator for a maneuver time of 
2.5 seconds are presented in Figure 4.6. and 4.7 respectively. Torque and 
momentum wheel speed plots for a maneuver time of 2.5 seconds are presented in 
Figures 4.8 and 4.9 respectively. 

Movement of the manipulator did not cause measurable disturbance to the 
main body. Manipulator torques were smoothly applied, and the control system 
was able to counter these torques effectively and in a timely manner, negating 
motion by the main body. The stability of the main body during manipulator 
motion is desirable since it will reduce pointing errors for unstablized imaging 
devices or high gain antenna communications systems. 

The polynomial reference controller demonstrated stable operation over a 
wide variety of operating conditions and feedback gains. The magnitude of the 
gains had little or no effect upon the maneuver time since this characteristic was 


specified in the reference maneuver. Commanded torques closely tracked the 
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Figure 4.6 Polynomial Reference Tracking Controller Joint Position Time 
History With Gp=1, Gy=5 And Maneuver Time Of 2.5 Seconds 
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Figure 4.7 Polynomial Reference Tracking Controller Joint Velocity Time 
History With Gp=1, Gy=5 And Maneuver Time Of 2.5 Seconds 
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Figure 4.8 Polynomial Reference Tracking Controller Torque Time History 
With Gp=1, Gv=5 And Maneuver Time Of 2.5 Seconds 
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Figure 4.9 Polynomial Reference Tracking Controller Momentum Wheel 
Speed Time History With Gp=1, Gy=5 And Maneuver Time Of 2.5 Seconds 
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reference torques with no discernible difference between the reference and 
commanded torques. The polynomial reference tracking controller provided 
smooth position control commands for all maneuvers. With the tracking 
controller, there were no discontinuities or sudden changes in torque commands. 
The performance was independent of the magnitude of the commanded maneuver 
with no effect of gain selection being noted. Maneuver time could be specified 
but was subject to actuator and structural characteristics. Feedback errors 
remained small throughout the maneuver as the controller smoothly tracked the 
reference. This resulted in smoothly changing torque commands with smooth and 


predictable motion of the manipulator. 


D. NEAR-MINIMUM-TIME REFERENCE TRACKING CONTROLLER 

The theory behind the near-minimum-time tracking controller is described in 
Chapter III and [Ref. 1, 2]. Two cases for this controller were selected. In the first 
case, Q was set equal to 0.25. The shape function generated by this reference are 
sinusoidal in shape. In the second case, an @ of 0.1 was used. As Q approaches 
zero, the input torque shape approached that of a square wave with a period of 
the maneuver time. This shape more closely approximated the minimum time 
bang-bang controller. A maneuver times of 2.5 seconds was used so that the 
polynomial reference and the near-minimum-time control system performance 
characteristics could be compared. Similar plots with a maneuver time of 5 
seconds can be found in Appendix D. 

The general performance of the near-minimum-time reference tracking 
controller provided some of the advantages and disadvantages of the two 


previous control systems. Since this was a tracking controller and the feedback 
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errors were generally small, the commanded torques were generally smooth with 
only minor disturbances to the main body. 

Near minimum-time reference position and velocity time history of the 
manipulator for a maneuver time of 2.5 seconds with © = 0.25 are presented in 
Figure 4.10. and 4.11 respectively. Momentum wheel speed plots for a maneuver 
time of 2.5 seconds with @ = 0.25 are presented in Figures 4.12 and 4.13 
respectively. Near minimum-time reference position and velocity time history of 
the manipulator for a maneuver time of 2.5 seconds with & = 0.1 are presented in 
Figure 4.14. and 4.15 respectively. Momentum wheel speed plots for a maneuver 
time of 2.5 seconds with & = 0.1 are presented in Figures 4.16 and 4.17 
respectively. 

With & = 0.25 the input torque shaping closely resembles that of the 
polynomial reference trajectory tracking controller. As with the polynomial 
reference trajectory, the manipulator links moved smoothly from the initial to the 
final conditions. Note that for this controller, there is only a very small angular 
displacement of the main body. 

Joint velocities for the manipulator links follow a smooth path, beginning and 
ending with zero velocities. With this controller, note that unlike the previous 
controller, there 1s no phase difference between the movement of the manipulator 
links. Both links move with the same velocity at the same time. The main body 


also displays a very small angular velocity. 
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Figure 4.10 Near-Minimum Time Tracking Controller Joint Position Time 
History With Gp=1 and Gy=5, Maneuver Time Of 2.5 Seconds , And © = 0.25 
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Figure 4.11 Near-Minimum Time Tracking Controller Joint Velocity Time 
History With Gp=1 and Gy=5, Maneuver Time Of 2.5 Seconds , And & = 0.25. 
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Figure 4.12 Near-Minimum Time Tracking Controller Torque Time History 
With Gp=1 and Gy=5, Maneuver Time Of 2.5 Seconds , And & = 0.25 
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Figure 4.13 Near-Minimum Time Tracking Controller Wheel Speed Time 
History With Ср=1 and Gy=5, Maneuver Time Of 2.5 Seconds , And & = 0.25 
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Figure 4.14 Near-Minimum Time Tracking Controller Joint Position Time 
History With Gp=1 and Gy=5, Maneuver Time Of 2.5 Seconds , And & = 0.1 
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Figure 4.15 Near-Minimum Time Tracking Controller Joint Velocity Time 
History With Gp=1 and Gy=5, Maneuver Time Of 2.5 Seconds , And & = 0.1 
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Figure 4.16 Near-Minimum Time Tracking Controller Torque Time History 
With Gp=1 and Gy=5, Maneuver Time Of 2.5 Seconds , And & = 0.1 
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Figure 4.17 Near-Minimum Time Tracking Controller Wheel Speed Time 
History With Gp=1 and Gy=5, Maneuver Time Of 2.5 Seconds , And & = 0.1 
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As G, goes toward zero, the shape of the input torque reference becomes 
more square. The square corners of the torque input are difficult for the controller 
to track and required larger gains. This characteristic is best noted in the previous 
depicted plots with & = 0.1. Note that rapid changes in the wheel speed are 
required. Even with large gains, the main body was still disturbed by motions. 
The Jerky motion of the manipulator disturbed the main body and resulted in small 
but detectable position changes. Asin the previous controllers, any disturbance 
to the main body can degrade communications links or cause pointing errors for 


optical imaging devices. 
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V. CONCLUSIONS 


Three different control systems were simulated and analyzed. All three 
controllers were stable and were able to position the manipulator in a timely and 
effective manner. The most significant difference between the controllers turned 
out to be how effective the control system was stabilizing the main body while 
positioning the manipulator. 

The linear feedback controller was the simplest controller conceptually and 
the easiest to implement. This controller provided stable control over a wide 
range of gains and for a wide variety of maneuvers. Large gains were required to 
achieve acceptable levels of performance. Large gains corresponded to large 
torques and large displacements of the main body. Position and velocity gains 
had to be selected to balance control torques, system performance, and motion of 
the main body. As the control system was implemented, it was not possible to 
achieve all of these objectives for the given reference maneuver. 

The polynomial reference trajectory for robotic applications represents a 
classıc approach for generating a reference trajectory to position and control a 
two link manipulator. This approach also offers the advantage that the reference 
trajectory can be pre-calculated off line prior to the maneuver. These pre- 
calculated values for the reference trajectory can be used in conjunction with the 
controller to minimize real time computational requirements. 

The polynomial reference tracking controller provided smooth position 
control commands for all maneuvers. With the tracking controller, there were no 
discontinuities or sudden changes in torque commands. The performance was 


independent of the magnitude of the commanded maneuver and only slightly 
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affected by gain selection. Maneuver time could be specified but was subject to 
actuator and structural characteristics. Feedback errors remained small 
throughout the maneuver as the controller smoothly tracked the reference. This 
resulted in smoothly changing torque commands with smooth and predictable 
motion of the manipulator. The polynomial reference tracking controller was the 
most effective controller implemented with respect to accurate and timely 
positioning of the manipulator, and stabilization of the main spacecraft body. 

The general performance of the near-minimum-time reference tracking 
controller provided some of the advantages and some of the disadvantages of the 
two previous control systems. Since this was a tracking controller and the 
feedback errors were generally small, the commanded torques were generally 
smooth with only minor disturbances to the main body. 

The near-minimum-time for the reference maneuver was determined by the 
maximum torque capability of each actuator and the magnitude of the maneuver. 
The maximum of the three times calculated for each of the three joints was 
specified as the maneuver time. For each maneuver, one joint would then become 
the limiting case for the selected maneuver based on these requirements. Two of 
the three remaining actuators therefore would operate at less than the maximum 
toque capability in order to complete the maneuver at the same time as the other 
links completed the maneuver. 

Large gains were required to force the controller to closely track the 
reference input as the input torque shape approached the minimum maneuver 
time. Even with larger gains, the controller was not able to completely negate the 
torques generated by the manipulator and some small rotations of the main body 


were observed. 
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Two different control laws were evaluated. In the first, constant gains with 
linear feedback was utilized. This controller proved to be the easiest to implement 
but did not effectively stabilize the main body during movements by the 
manipulator. The second control law entailed using a tracking controller with 
constant gains. The reference tracking controller represents a traditional and 
successful way of positioning the manipulator. With the reference tracking 
controller two different references were used. In the first a polynomial was used 
to generate a reference trajectory. This technique proved to be only slightly more 
complex than the linear feedback controller with constant gains, was dependent 
upon mass and inertia characteristics of the manipulator and was the most 
effective at positioning the manipulator while minimizing the motion of the main 
body. In the second, a near-minimum-time technique was employed to generate a 
reference trajectory. This reference was more complex, but provided the 
capability to position the manipulator in near-minimum-time. This technique 
provided greater flexibility in shaping the input torque reference but was not as 
successful as the polynomial reference tracking controller in stabilizing the main 
body. In the end, the polynomial reference tracking controller provided the best 


performance of the three controllers simulated. 


A. SUBJECTS FOR FUTURE RESEARCH 

Lyapunov theory represents only one methodology for nonlinear control 
system design. Many other methodologies including neural networks, adaptive 
controllers, sliding controllers, and robust controllers among others provide 
additional areas for research. 

With the addition of the vision server to the Spacecraft Robotics Simulation 


Lab, the capability to track a target as well as the endpoint of the manipulator will 
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become available. This provides the capabilty to perform end point tracking 
tasks. Once this is successful with a stationary target, the same can be repeated 
with a moving target. 

The design of the manipulators allows for the replacement of the rigid 
manipulator arm design with a flexible arm design. Control system design for 
accurately and quickly positioning the flexible manipulator provides another area 
for future research. 

The minimum time to complete the maneuver was the longest of the three 
joint maneuver times and was based on the maximum torque capabilities of the 
actuator and the magnitude of the maneuver. With the near-minimum-time 
reference tracking controller, the switching time for the maneuver was specified 
to be one half of the maneuver time. This methodology resulted in only one of 
the three actuators working at maximum torque. By parameterizing the near- 
minimum-time equations in another way, it may be possible to optimize the 
solution in terms of a variable switching time and variable maneuver time for each 
of the actuators. This may allow optimization of the torques or time required to 


perform the maneuver, maneuver time or torque requirements. 
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APPENDIX A 


TABLE A.1 Servo Motor Characteristics 


Manipulator Momentum 
Characteristic Units Actuator Wheel 
Actuator 


[ModelNumber | | 9FGHD | IRIGMACH-1_ 
Gear Reduction Ratio | | м | mo 
Rated Torgue | inchpoua | so | 3185 _ 
1.4 
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APPENDIX B 


A. SYSTEM KINETIC ENERGY 
1. Body 1 Kinetic Energy 


Ty = 1 0,2 (В.1) 


2. Body 2 Kinetic Energy 
‚2 о 
Т2 = 5 ДЕ; ;m Li 6; 
3 mj Li Logs; 01 02 cos (021) (B.2) 


T ? 112 Eon: Dr 


3. Body 3 Kinetic Energy 
222 252 2022 2287: 
Тз = E ста Өз 4 5 m3 12 0; 120; -12,, 0 | 
+ m3 [1 12 0, Ө, cos (91) + m3 Їл Гета 0, Ө; cos (041) (В.3) 
+ m3 L2 Le.m., 02 03 cos (032) 


4. Total Kinetic Energy 
3 
ToT: (B.4) 
iz] 


B. EQUATIONS OF MOTION 
1. Mass Matrix 
The mass matrix is a function of the mass and inertia characteristics of the 
system as well as the geometry of the system. The mass matrix is defined in 
equation (B.5) where the elements of the mass matrix are described as follows in 


equations (B.6) through (B.11). 


Э5) 


111 0112 П113 
М(0) = | по 0122 1023 
113] 11132 11133 


where 


Eo 


mj; 2 Ij 4 Li [my mo 4 mj] + my 
2 Lem 9 
mp2 7 lcm; * 12 m [Ens + ПВ 


2 
m33 = km, d Lem; ПВ 





mi2 = = 14 10 соѕ(0›1) m [es 
2 


| + 13 | 
туз = 31 = 14 ем, С0$(@31} пз 
m3 =m2 = L? Lem, со$(Өз2) m3 


2. Coriolis And Centrifugal Acceleration Terms: 


G(1) 
G(2) 
G(3) 


С(0,0) = 








G(1)=[ (ma Lı Lem.)+ (m3 Lı L2)] sin (021) 63 + 
(ma L; Lc.m.,) sin (831) 03 


6(2) =- [ (по Та Log.) (ms Li Lz)] sin (62:) 81 « 
(ms L2 Lc) sin (031) 03 


G(3) =- (m3 Ly Lem.) 40 (027) 61 -(m3 Lz Lem.) sin (032) 65 
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(B.5) 


(B.6) 


(B.7) 


(B.8) 


(B.9) 


(B.10) 


(B.11) 


(B.12) 


(B.13) 


(B.14) 


(B.15) 


C. POLYNOMIAL REFERENCE TRAJECTORY 


1. Vector Polynomial Describing Tip Position: 


r =T (to) + flt) [г (ty) - r (to) (B.16) 
r= b Meter el 
tr- to 
f[t) =c1 + c2 T + c3 Z? + c4 TŠ + cs T + c6 T> (B.17) 


2. Boundary Conditions And Polynomials 


(0)=0,{0)=0, 0) =0 
(1)-1,41)-0,Ң1)-0 
f(t)210 5? - 15 4* - 6? (B.18) 
f(T) = 30 12 - 60 13 + 30 14 (B.19) 
f(t) =60 7 - 180 12 + 120 73 (B.20) 
3. Vector Position 
T(t) = T (to) + f (1) [r (te) - v (to) (B.21) 
4. Vector Velocity 
T (1) = f (1) led eh (B.22) 
tr - to 
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5. Vector Acceleration 


Ебу = Ë сыы 


(tr - to) 


D. TWO LINK INVERSE KINEMATICS 
Law Of Sınes & Cosines: 


х2 +у2 =1] +15 + 21 12 со (6 - 01) 
Өз = 9> - 91 


2 2 12 12 
х5 t y5- 11-1 
cos (05; ) 2 22—22 —1 2 a б : 


sin (9.1 ) = + ¥ I- cos (021) 


Өзү = аќап2 paren 
cos (051 ) 


Velocity Vector: 
г-1н 9) 


- l> sin (02), = Із sin (Өз) 
lo COS (0), 13 COS (Өз) 


DIE 


Acceleration Vector: 


г= [н] 0] + [8] 0) 
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(B.23) 


(B.24) 


(B.25) 


(B.26) 


(В.27) 


(В.28) 


(В.29) 


(В.30) 


(В.31) 


H- = lo COS (6), = la COS (03) (В.32) 
= b sin (0), = Із sin (Өз) 
E. NEAR MINIMUM TIME RIGID BODY MANEUVER 


1. Near-Minimum-Time Maneuver 


I Oref = Usef = Umax f(At, tet) (B.33) 
I - Moment Of Inertia About Axis Of Rotation 
tf - Maneuver Time 
21-21! 
b —t- A 
t3 = tr - At 
At = Q t¢ = Rise Time 
t; = B tr = Switching Time, where B = 0.5 
2. Torque Shaping Function 


-( з 28 for 0 <t < At 
At At 


= J| for Af <t <U 
2 
ride o1 2 esf po 


= - ] for to <t < t> 


__| «esf o - oe 
At At 


| fort; <st< LU 








(B.34) 


| for t3 St « trj 








{ 


9,4) = Oo 4 еа | САО ае (В.35) 


to 
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t 


O, ((t) = 90 + Ө, (t-to) + PES | f(At,t¢,T2) dt2 dt (B.36) 
to 


to 


3. Boundary Conditions 


to = 0; 60)- 060 60)-0 
t = tr; Ө(=Ө‹ 94-0 
Solution for the piece wise integration of equations (B.35) and (B.36) for 
the given time interval provides us with refeence angular displacements and 
angular velocity. 


4. Reference Angular Displacements And Velocities 





O<t<At 
мече Late a 2 
КЕТІП 44 

At <t <t! 
Oef = a 1 tae J tAt+ sA (At) | (В.39) 
бес = = t % At | (B.40) 
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mets t 
2302 3 о. + 1] + 
20 4 8 
O, = max аи ыы, 
2 


I 2At! 2122) 5124 


| 1 tp - 3 S AU E | 


3 4 
бы- zs [s Lace n [etl o „(шј 
I 2 2At 2At 2At 


21 Газ +] (у - За: t - t2] ... 
Ex 4 8 f 2 Ч | 2) 
2 
I | А|-1-А4 | 
212 


Örer = Ymax |-t + ti + t2 -L At | 





[3S ts tr 


- 152 - 1 «11:44 At 
ш yet alte ta Atle БА 


эӊ + | 


• 3 Í 
às ts LL Ace At. ы), МЯЛ „ен 
1 2 Аї At 2221 


Um 


Ё 


ref = 


ме | 
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(8.441) 


(B.42) 


(B.43) 


(B.44) 


(B.45) 


(B.46) 


At time t = tf; 


1.1 + Lo? 
Orer = Umax р 27 Ss 
бег = О 


5. Near-Minimum-Time Relationships 


са 1 (0, - 60) 
| u n Lort о2 
ae dee 10 
I; (Of. - 8p, 
= 1 | [| ө) 


ра 
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(В.47) 


(B.48) 


(B.49) 


(B.50) 


APPENDIX С 


A. LINEAR FEEDBACK MATLAB SIMULATION PROGRAM 


1. 


Main Program For Linear Feedback Simulation 
% Constants 

% Lenght of Manipulators 

L(1) = 15*2.54/100; 

1(2) = 17*2.54/100; 

L(3) 2 17*2.54/100; 

L(4) = 20/100; 

% Distance From Axis Of Rotation To Center Of Mass 
Lcm(1) 2 0/100; 

Lcm(2) = 36.45/100; 

Lcm(3) = 34.9/100; 

% Mass Of Major Components (kg) 

m(1) = 54.69; 

m(2) = 2.09364; 

m(3) = 2.466; 

m(4) = 10.667; 

% Inertias Of Major Components (kg-m^2) 
I(1) 2 4.32132; 

I(2) 2 3.20338e-2; 

155 5:383986:2: 

I(4) = 0.0912; 
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% Integrate Equations Of Motion Using Commercial 

% Matlab Runge-Kutta 4 Routine 

% Boundary Conditions 

tO = 0; 

tfinal = 40; 

x0 = [0.0; 0.0; 0.0; 0*pi/180; 20*pi/180; 40*pi/180]; 

(01 = 1е-8; 

trace=1; 

Call "Ifbrk" Function And Integrate Equations Of Motion 

[t,x,uu] 2 rku2(Ifbrk ,tO,tfinal,xO,tol,trace); 

% Program To Calculate Motor Torque, Current, and Voltage 

ТЕ-1.41е-2; % N-m 

КТ=2.28е-2; % N-m/amp 

KD=2.00573e-5; % N-m/rad/sec 

КЕ=2.28271е-2; % Volt/rad/sec 

RT=0.95; % ohms 

j=length(t) 

Гог 1=1:]; 
amp22(i) = (uu(1,2)/148.51 + TF + КГ * х(1,2)* 148.51 ) / КТ; 
volt22(1) 2 КЕ * х(1,2) + КТ * атр22(1); 
amp33(i) = (uu(i,3)/148.51 + TF + KD * x(i,3)*148.51 )/ KT; 
у01133(1) = КЕ * х(1,3) + КТ * атр33(1); 


епа 
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% Program To Calculate Motor Torque, Current, and Voltage 
% For Momentum Wheel 
=0.0777; %N-m/amp 
KD-25.29131e-4; 96 N-m/rad/sec 
KE=0.48711; % Volt/rad/sec 


RT=1.4; Zo ohms 

Iw=0.0912; % kg-m^2 
thd0=104.7; % rad/sec (= 1000rpm) 
for i=1:j; 


tor(i) = uu(i, 1); 
thddw(i) = tor(i)/Iw; 
thdw(i) = thddw(i) * t(i) + thd0; 
amp(i) = (tor(i) + TF + KD * thdw(i) ) / KT; 
volt(i) = KE * thdw(i) + RT * amp(i); 
x1(1)2L(1)*cos(x(1,4)); 
у1(1)=1(1)*11(х(1,4)); 
x2(1)2x1()-L(2)*cos(x(1,5)); 
y2()-yla)LO)*sin(x(1,5)); 
x3(1)2x2(0)-L(3)*cos(x(1,6)); 
y3()zy2Q)*LG)*sin(x(,6)); 
end 
% Store Data For Plotting Later 
datal=[t,x*180/pi,uu]; 
data2=[t,x1',y1',x2',y2',x3',y3/; 
data32[t,thdw'*30/pi,thddw'*30/pi]; 
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data4=[t,amp',amp22',amp33',volt’,volt22',volt33'); 
% Save Data In Text Format 

save Ifb9.dat datal /ascii 

save Ifb10.dat data2 /ascii 

save Ifb11.dat data3 /ascii 


save Ifb12.dat data4 /ascii 
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| 





2. Linear Feedback Equation Of Motion Function 


% Function Containing Linear Feedback Equations Of Motion 
function [xdot,U1] z Ifbrk(t,x) 

% Constants 

% Lenght of Manipulators 

L(1) = 15*2.54/100; 

L(2) = 17*2.54/100; 

L(3) = 17*2.54/100; 

L(4) = 20/100; 

9% Distance From Axis Of Rotation To Center Of Mass 
Lem(1) = 0/100; 

Lem(2) = 36.45/100; 

Lem(3) = 34.9/100; 

% Mass Of Major Components (kg) 

m(1) = 54.69; 

m(2) = 2.09364; 

m(3) = 2.466; 

m(4) = 10.667; 

% Inertias Of Major Components (kg-m2) 
1) 4232162: 

I(2) = 3.20338e-2; 

I(3) = 5.38398e-2; 

I(4) = 0.0912; 
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% Angles 

thd(1) = x(1); 

thd(2) = x(2); 

thd(3) = x(3); 

% Convert Input Variables Into Variable Used In This Function 
th(1) = x(4); 

th(2) =x(5); 

(3) = х(6); 

% Final Conditions For Manipulator And Main Body 
thf1=0; 

thf2=40*pi/180; 
thf3260*p1/180; 

thdf120; 

thdf2=0; 

thdf3=0; 

% Linear Feedback Control Law 
glp=.1; 

g2p=.1; 

g3p=.1; 

glvz.2; 

g2v=.2; 

23у=.2; 
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% Control Torques 

U(3)=-g3p*(th(3)-thf3)-g3v*(thd(3)-thdf3); 
U(2)=U(3)-g2p*(th(2)-thf2)-g2v*(thd(2)-thdf2); 

U(1)ZU(2)-gl p*(th(1)-thf1)-glv*(thd(1)-thdf1); 

0140 

% Calculate Coeffients For The Equation Of Motion And Integrate 
[MM,GM] = mgm(th,thd); 

B = [1,-1,0;0,1,-1;0,0,1); 

thdd=inv(MM)*(B*U1-GM’); 

xdot = [thdd;x(1);x(2);x(3)]; 
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B. POLYNOMIAL REFERENCE TRACKING CONTROLLER 


1. 


Polynomial Reference Maneuver Tracker Main Program 
9% EOM3.m Program 

% Constants 

% Lenght of Manipulators 

L(1) = 15*2.54/100; 

Ї (2) = 7254/1900: 

L(3) = 17%2.54/100; 

L(4) = 20/100; 

% Distance From Axis Of Rotation To Center Of Mass 
Lem(1) = 0/100; 

Lcm(2) z 36.45/100; 

Lcm(3) z 34.9/100; 

J Mass Of Major Components (kg) 

m(1) = 54.69; 

m(2) = 2.09364; 

m(3) = 2.466; 

m(4) = 10.667; 

% Inertias Of Major Components (kg-m‘2) 
I(1) 2 4.32132; 

I(2) 2 3.20338e-2; 

I(3) 2 5.38398e-2; 

I(4) 2 0.0912; 
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% Boundary Conditions And Integration Time 

tO = 0; 

tfinal = 10; 

х0 = [0.0; 0.0; 0.0; 0*р1/180; 20*р1/180; 40*р1/180]; 

tol = le-8; 

trace=1; 

% Integrate Equations Of Motion 

[t,x,uu) = rku2(‘eom3rk ,tO,tfinal,x0,tol trace); 

j=size(t); 

% Calculate Electrical Power Requirement For Manipulator Actuators 


% Motor Parameters 


TF=1.8e-2 % N-m 

КТ-2.28е-2 % N-m/amp 
KD=2.00573e-5 % N-m/rad/sec 
кЕ=2,2627[е-2 % Volt/rad/sec 
КТ=0.95 % ohms 

j=size(t) 

for i=1;j; 


агар22(1) = (uu(2,1)/148.51 + TF + К” * х(1,2) ) / КТ; 
volt22(1) 2 KE * x(1,2) + RT * amp22(1); 
amp33(i) = (uu(3,1)/148.51 + TF + KD * x(1,33) )/ KT; 
volt33() = KE * x(i,3) + RT * amp33(i); 

end 

7o Program To Calculate Motor Torque, Current, and Voltage 


% Momentum Wheel 
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% Motor Parameters 


TF=0.0777 % N-m 

KT=0.48732 % N-m/amp 
KD=5.29131e-4 7o N-m/rad/sec 
КЕ=0.48711 % Volt/rad/sec 

RT=1.4 % ohms 

Iw=0.0912 7b kg-m^2 

thd0=104.7 % rad/sec (= 1000rpm) 
j=size(t) 

for i=1:j; 


tor(i) = uu(l |); 

thddw(1) z tor(3)/Iw; 

thdw(1) 2 thddw(1) * t(1)  thdO; 

amp(i) = (tor(i) - TF -- KD * thdw(1) )/ KT; 
volt(i) 2 KE * thdw(1) - RT * amp(i); 


end 
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2. Polynomial Reference Tracking Equations Of Motion Function 
function [xdot,U1] 2 eom3rk(t,x) 
% Constants 
L(1) 2 15*2.54/100; 
L(2) 2 17*2.54/100; 
L(3) = 17*2.54/100; 
L(4) = 20/100; 
Lem(1) = 0/100; 
Lem(2) = 36.45/100; 
Lcem(3) = 34.9/100; 
Jo Mass 
m(1) = 54.69; 

m(2) = 2.09364; 
m(3) = 2.466; 
m(4) = 10.667; 

% Inertia 

I(1) = 4.32132; 
(2) = 3.20338e-2; 
КЗ) = 5.38398е-2; 
(4) - 0.0912; 

7o Angles 

thd(1) = x(1); 
thd(2) 2 x(2); 
thd(3) 2 x(3); 
(1) = х(4); 
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th(2) = x(Š); 

th(3) = x(6); 

[MM,GM] = mgm(th,thd),; 

% Gains 

glp=1.0; 

g2p=1.0; 

g3p=1.0; 

glv=5.0; 

у= О 

g3v=5.0; 

[Uref,thref,thdref,thddref] = eom3ref(t,L,Lcm,m,)D; 
dU(3)=-g3p*(th(3)-thref(3))-g3 v*(thd(3)-thdref(3)); 
dU(2)2dU(3)-g2p*(th(2)-thref(2))-g2v*(thd(2)-thdref(2)); 
dU(1)2dU(2)-glp*(th(1)-thref(1))-g1 v*(thd(1)-thdref(1)); 
Ul=Uref+dU', 

О-01: 

В =[1,-1,0;0,1,-1;0,0,1]; 

thdd=inv(MM)*(B*U'-GM’); 

xdot = [thdd;x(1);x(2);x(3)]; 
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Polynomial Reference Trajectory Function 
function [Uref,thref,thdref,thddref}] = eom3ref(t,L,Lcm,m,]); 
thref(1)=0; 


% Initial And Final Time For Maneuver 
(0 = 0: 
t = 10; 


% Initial And Final Vector Positons 

r3x0 = L(1 )*cos(thref(1))+L(2)*cos(20* pi/1 80)+L(3)*cos(40* pi/1 80); 
r3y0 = L(1)*sin(thref(1))+L(2)*sin(20* pi/180)+L(3)*sin(40* pi/1 80); 
r3xf = L(1)*cos(thref(1))+L(2)*cos(40*pi/180)+L(3)*cos(60*pi/180); 
r3yf = L(1)*sin(thref(1))+L(2)*sin(40* pi/1 80)+L(3)*sin(60*pi/180); 


% Calculate Reference Maneuver 

tau = ( t - t0)/(tf - t0); 

r3x = r3x0 + ( r3xf - r3x0 ) * ( 10 * tau^3 - 15 * tau^4 + 6 * tau^5 ); 

r3y = r3y0 + ( r3yf - r3y0 ) * ( 10 * tau^3 - 15 * tau^4 + 6 * tau^5 ); 
r3xd = ( r3xf - r3x0 )/ (tf - t0 )* ( 30 * tau^2 - 60 * tau^3 + 30 * tau^4); 
r3yd = ( r3yf - 13y0 ) / ( t£ - t0 )*( 30 * tau^2 - 60 * tau^3 + 30 * tau^4); 
r3xdd = (r3xf-r3x0)/((tf-t0)*%2)*(60* tau- 180*tau*2+ 120*tau%3); 

r3ydd = (r3yf-r3 y0)/((tf-t0)*2)*(60* tau-180*tau*2+120*tau3); 
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if (ttf); 
r3x=0; 
r3y=0; 
r3xd=0; 
r3yd=0; 
r3xdd=0; 
r3ydd=0; 


end 


7o Determine Inverse Kinematics 

th23 = kink(r3x-L(1)*cos(thref(1)),73y-L(1)*sin(thref(1),L(2),L(3)); 
thref(2) = th23(1); 

thref(3) = 1230); 


% Calculate Joint Velocites Using Jacobean 
H = [-L(2)*sin(thref(2)),-L(3)*sin(thref(3));.... 
L(2)*cos(thref(2)), L(3)*cos(thref(3))]; 
thd23 = inv(H) * [ r3xd; r3yd J; 

thdref(2) = thd23(1); 
thdref(3) = thd23(2); 


% Calculate Joint Acceleration Using Jacobean 


Hdot=[-L(2)*thdref(2)*cos(thref(2)),-L(3)*thdref(3)*cos(thref(3));.... 
-L(2)*thdref(2)*sin(thref(2)),-L(3)*thdref(3 )* sin(thref(3))]; 
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thdd23=inv(H)*([r3xdd;r3ydd]-Hdot*[thdref(2);thdref(3)]); 
thddref(2) = thdd23(1); 

thddref(3) = thdd23(2); 

% Calculate Reference Control Torques 

[MM,GM] = mgm(thref,thdref); 

B =[1,-1,0;0,1,-1;0,0,1); 

Uref = inv(B) * (MM * thddref + GM’ ); 


py 


C. NEAR-MINIMUM-TIME RERENCE TRACKING CONTROLLER 


1. 


Main Program 

% nmtl program 

% Constants 

1 (1) = 15*2.54/100: 
L(2) = 17*2.54/100; 
L(3) = 17*2.54/100; 
L(4) = 20/100; 
Lem(1) = 0/100; 
Lem(2) = 36.45/100; 
Lem(3) = 34.9/100; 
% Mass 

m(1) = 54.69; 

m(2) = 2.09364; 
m(3) = 2.466; 

m(4) = 10.667; 


% Inertia 

I(1) 2 4.32132; 

I(2) 2 3.20338e-2; 
I(3) 2 5.38398e-2; 
(А) - 0.0912; 

% Maximum Torque 
umax=0.300; 


% Shaping Function Coefficient 
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alfa=0.1; 

beta=0.5; 

(0=0; 

MM(1,1) = 1(1)+L(1)*2*(m(2)4+m(3))+L(4)42*m(4); 
MM(2,2) = I(2) + m(2) * Lem(2)42 + m(3) * L(2)42; 
MM(3,3) = 1(3) + m(3) * Lem(3)^2; 


% Initial And Final Manipulator Position 
thO=[0*pi/180;20*pi/180;40* pi/1 80]; 
thf=[0.01 *pi/180;40*pi/180;60* pi/1 80); 


% Determine Minimum Time To Perform Maneuver 

T(1)2sqrt(MM(1,1)*(thf(1)-thO(1))/... 
(umax(1)*(1/4-1/2*alfa*1/10*alfa^2))); 

T(2)2sqrt( MM (2,2) *(thf(2)-thO(2))/... 
(umax(1)*(1/4-1/2*alfa1/10*alfa^2))); 

T(3)=sqrt(MM(3,3)*(thf(3)-th0(3))/... 
(umax(1)*(1/4-1/2*alfa+1/10*alfa”2))) 

T_max=max(T) 

tf=T_max 

delt=alfa*tf 

tf=T_max 

dt=alfa*tf 

ts=beta*tf 


tl=ts-dt 
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t2=ts+dt 

t3=tf-dt 

echo off; 

(0=0; 

trace=1; 

tol=le-8; 

x0=[0;0;0;0* pi/180;20* pi/180;40* pi/1 80); 


% Integrate Equations Of Motion 


[t,x,uu] 2 rku2( nmtrk' ,tO,tf,xO,tol,trace); 
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Near-Minimum-Time Equations Function 


function [xdot,U] = nmtrk(t,x); 


umax(1)=0.300; 


% Constants 


L(1)= 15*2.54/100; 


L(2) = 17*2.54/100; 


L(3) 2 17*2.54/100; 


L(4) = 20/100; 


Lem(1) = 0/100; 


Lcm(2) = 36.45/100; 


Lcm(3) z 34.9/100; 


% Mass 


m(1) = 54.69; 
m(2) = 2.09364; 
m(3) = 2.466; 
m(4) = 10.667; 


% Inertia 


83 


I1): 223921532: 
IO) UBRO o 
I(3) = 5.38398e-2; 
I(4) = 0.0912; 


% Angles 


thd(1) = x(1); 
thd(2) = x(2); 
thd(3) = x(3); 


(1) - х(4); 
КО) =х0); 
(3) =х(6); 


[MM,GM] = mgm(th,thd); 


% Gains 
glp=.10; 
g2p=.10; 
g3p=.10; 
glv= 50; 
а2у= 5 
g3v=.50; 
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[MM,GM] = mgm(th,thd); 


[Uref,thref,thdref,thddref] = nmt_ref(t); 


dU(3)=-23p*(th(3)-thref(3))-23v*(thd(3)-thdref(3)); 
dU(2)-dU(3)-g2p*(th(2)-thref(2))-g2v*(thd(2)-thdref(2)); 
dU(1)2dU(2)-g1p*(th(1)-thref(1))-g1v*(thd(1)-thdref(1)); 


U1=Uref+dU;; 

U=UT; 

В =[1,-1,0;0,1,-1;0,0,1]; 

xdot = [ inv(MM) * (B * U'- GM’); x(1); x(2); x(3) J; 
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3. Near-Minimum-Time Reference Function 
% Reference Generator Function 
function [Uref,thref,thdref,thddref] 2 nmt ref(t); 
umax(1)=0.300; 
% Constants 
L(1) 2 15*2.54/100; 
L(2) 2 17*2.54/100; 
L(3) 2 17*2.54/100; 
L(4) = 20/100; 
Lem(1) = 0/100; 
Lcm(2) = 36.45/100; 
Lem(3) = 34.9/100; 


% Mass 

m(1) = 54.69; 
m(2) z 2.09364; 
m(3) z 2.466; 
m(4) z 10.667; 

Ф Inertia 

I(1) 2 4.32132; 
1(2) = 3.20338е-2; 
I(3) 2 5.38398e-2; 
(4) - 0.0912; 
ММ(1,1) = К1)+1(1)^2*(11(2)+1(3))+1(4)^2*т(4); 
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MM(2,2) = 1(2) *- m(2) * Lcm(2)^2 + m(3) * L(2)^2; 

MM(3,3) = 1(3) + m(3) * Lcm(3)^2; 

thO=[0*pi/180;20*pi/180;40* pi/180); 

thf=[0.01 *pi/180;40*pi/180;60* pi/1 80]; 

alfa=0.1; 

T(1)2sqrt(MM(1,1)*(thf(1)-thO(1))/(umax(1)*... 
(1/4-1/2*alfa4-1/10*alfa^2))); 

T(2)2sqrt( MM(2,2)*(thf(2)-th0(2))/(umax(1)*... 
(1/4-1/2*alfa4-1/10*alfa^2))); 

T(3)2sqrt(MM(Q3,3)*(thf(3)-th0(3))/(umax(1)*... 
(1/4-1/2*alfa+1/10*alfa%2))); 

T_max=max(T); 

tf=T_max; 

dt=alfa*T_max; 

tl=tf/2-dt; 

t2=tf/2+dt; 

t3=tf-dt; 

umax(3)=MM(3,3)*(thf(3)-th0(3))/... 
(tf*2*(1/4-1/2*alfat+1/10*alfa’2)); 

umax(2)2MM(2,2)*(thf(2)-thO(2))/... 
(t£^2*(1/4-1/2*alfa4-1/10*alfa^2)); 

umax(1)2MM(1,1)*(thf(1)-thO(1))(tf^2*(1/4-1/2*alfa*- 1/10*alfa^2)); 


% Near Minimum Time Reference Maneuver 
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if (t>=0 & t<=dt); 

f=(t/dt)*2*(3-2*(t/dt)); 
ff=dt*((t/dt)*3-(1/2)*(t/dt)*4); 
fffz(dt^2)*((1/4)*(U/dt)^4-(1/10)*(Ud0^5); 


elseif (t>dt & t<=t1); 

[= 

ff=(t-(1/2)*dt); 
fffz((1/2)*t^2-(1/2)*t*dt-(3/20)*dt^2); 


elseif (tot] & t<=t2); 
[ =1-2*(((ї-(1)/(2*40))^2*(3-2*((ї-41)/(2*аї)))); 
ff = -(1/2)*dt+t1+2*dt*(((t-t1)/(2*dt))-2*((t-t1)/(2*dt))A3+... 
((t-t1)/(2*d0))^4); 
НЕ = ((23/20)*alfa^2-(3/A)*alfa-(1/8))*tf^2--(2*dt)^2*... 
((1/2)*((t-t1)/(2*d0)^2-(1/2)*((t-E1 (2*d0)^4... 
+(1/5)*((t-t1 )/(2*dt))*5)+((1/2)*tf-(3/2)*dt)*(t-t1); 


elseif (t>t2 & t<=t3); 

f--l; 

ffz-t-t1-2-(1/2)*dt-2*dt*(((t2-t1)/(2*dt))-... 
2*((t2-t1)/(2*d0)^3--((t2-t1)/(2*dt))^4); 

fffz(-(21/20)*alfa^24-(1/4)*alfa4- 1/8) *tf^24-... 
(1/2)*(tf-3*dt)*(t-2)-(1/2)*(t-(1/2)*tf-dt)^2; 
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elseif (t>t3 & t<=tf); 

f = -1+(((t-t3)/(dt))*2*(3-2*((t-t3)/dt))); 

ff=((1/2)*dt+dt*(-(t-t3 )/dt+((t-t3)/dt)*3-(1/2)*((t-3)/dt)*4)); 

fff=(-(1/20)* alfa*2-(1/2)*alfa+1/4)*tf*%2+(1/2)*dt*(t-t3)+... 
dt^2*(-(1/2)*((t-()/d0^2(1/4)*((t-E3)/)d0^4-(1/10) *((t-C3)/d0^5); 


elseif (t>tf); 
ї=0; 
#=0; 
fff=0; 


end 


thref(1)=(umax(1)/MM(1,1))*fff+th0d ); 
thref(2)=(umax(2)/MM(2,2))*fff+th0(2); 
thref(3)=(umax(3)/MM(3,3))*fff+th0(3); 


thdref(1 )=(umax(1)/MM(1,1))*ff; 
thdref(2)=(umax(2)/MM(2,2))*ff; 
thdref(3)=(umax(3)/MM(3,3))* ff; 


thddref(1 )=umax(1)*f/MM(1,1); 
thddref(2)=umax(2)*f/MM(2,2); 
thddref(3)=umax(3)*f/MM(3,3); 
[MM,GM] = mgm(thdref,thref); 
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В =[1,-1,0;0,1,-1;0,0,1]; 
Uref = inv(B) * (MM * thddref -- GM"' ); 
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D. MISCELLANEOUS FUNCITONS 
1. Equation Of Motion Coefficient Funciton 
function [MM,GM] = mgm(th,thd); 


% Constants 

L(1) = 15*2.54/100; 
L(2) 2 17*2.54/100; 
L(3) 2 17*2.54/100; 
L(4) = 20/100; 
Lem(1) = 0/100; 
Lem(2) = 36.45/100; 
Lem(3) = 34.9/100; 


% Mass 

m(1) = 54.69; 
m(2) = 2.09364; 
m(3) = 2.466; 
m(4) = 10.667; 


% Inertia 

[(1) = 4.32132; 
(2) = 3.20338е-2; 
(3) = 5.38398e-2; 
I(4) = 0.00912; 
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% Subroutine To Calculate Mass and "G" Matrix 
th21 = th(2) - th(1); 
th31 = th(3) - th(1); 
th32 = th(3) - th(2); 


7o 'Mass' Matrix 

MM(1,1) = I(1)+L(1)42*(m(2)4+m(3))+L(4)%2*m(4); 

MM(1,2) 2 ((m(2)*L(1)*Lcm(2))*(im(3)*L(1)*L(2)))*cos(th21); 
MM(1,3) = m(3)*L(1)*Lcm(3)*cos(th31); 


MM(2,1) = MM(1,2); 

MM(2,2) = I(2) + m(2) * Lem(2)^2 + m(3) * L(2)^2; 
MM(2,3) = m(3) * L(2) * Lcm(3) * cos(th32); 
MM(3,1) = MM(1,3); 

MM(3,2) = MM(2,3); 

MM(3,3) = 1(3) + m(3) * Lem(3)%2; 


% 'G' Matrix 


2122 = ((m(2)*L(1)*Lem(2))+(m(3)*L(1)*L(2)))*sin(th21); 
c133 = m(3)*L(1)*Lom(3)*sin(th31); 


c211 =- ((m(2)*L(1)*Lem(2))+(m(3)*L(1)*L(2)))*sin(th2 1); 
с233 =  m(3)*L(2)*Lcm(Q)*sin(th32); 
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c311 =- m(3)*L(1)*Lem(3)*sin(th31); 
c322 z - m(3)*L(2)*Lem(3)*sin(th32); 


c1=[0,0,0;... 
0 .c122 0.... 
0,0,c133]; 


22-1221120:0:2 
0,0,0;... 
0.0.2385]. 


63=(c3 11-0:0:... 
O c322 O. 
0,0,0]; 


GM(1) = thd * cl * thd’; 


GM(2) = thd * c2 * thd; 
GM(3) = thd * c3 * thd’; 
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Inverse Kinematics Function 
function theta=kink(x,y,L1,L2) 


% Subroutine To Determine Inverse Kinematic Solution 


cl2=(xA2+yA2-L1A2-L 2227 0 И ЛЕ 
sl2=sqrt(1-c12⁄2); 

thetal2=atan2(s12,c12); 
minv=inv([L1+L2*c12,-L2*s12;L2*s12,L1+L2*c12]); 
c=minv*[x;y]; 

theta(1)=atan2(c(2),c(1)); 

theta(2)=theta(1)+thetal2; 
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APPENDIX D 


A. LINEAR CONSTANT GAIN FEEDBACK CONTROLLER 
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Figure D.1 Linear Constant Gain Feedback Joint Position Time History With 
Ср=0.1 Апа Су=0.2 
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Figure D.2 Linear Constant Gain Feedback Joint Velocity Time History 
With Gp=0.1 And Gy=0.2 
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Figure D.3 Linear Constant Gain Feedback Torque Time History With 
Gp=0.1 And Gy=0.2 
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Figure D.4 Linear Constant Gain Feedback Momentum Wheel Speed Time 
History With Gp=0.1 And Gy=0.2 
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B. POLYNOMIAL REFERENCE TRACKING CONTROLLER 
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Figure D.5 Polynomial Reference Tracking Controller Joint Position Time 
History With Gp=1 and Gy=5 And Maneuver Time Of 5 Seconds 
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Figure D.6 Polynomial Reference Tracking Controller Joint Velociy Time 
History With Gp=1 and Gy=5 And Maneuver Time Of 5 Seconds 
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Figure D.7 Polynomial Reference Tracking Controller Torque Time History 
With Gp=1 and Gv=5 And Maneuver Time Of 5 Seconds 
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Figure D.8 Polynomial Reference Tracking Controller Momentum Wheel 
Speed Time History With Gp=1 and Gy=5 And Maneuver Time Of 5 Seconds 
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C. NEAR-MINIMUM-TIME REFERENCE TRACKING CONTROLLER 
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F igure D.9 Near-Minimum Time Tracking Controller Joint Position Time 
History With Gp=1 , Gy=5, Maneuver Time Of 5 Seconds ‚ Апа © = 0.25 
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Figure D.10 Near-Minimum Time Tracking Controller Joint Velocity Time 
History With Gp=1 , Gy=5, Maneuver Time Of 5 Seconds , And a z 0.25 
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Figure D.11 Near-Minimum Time Tracking Controller Torque Time History 
With Gp=1 , Gy=5, Maneuver Time Of 5 Seconds , And & = 0.25 


105 


= 
А 
= 
š 
un 
2 
> 


Time (Sec) 





Figure D.12 Near-Minimum Time Tracking Controller Wheel Speed Time 
History With Gp=1 , Gy-5, Maneuver Time Of 5 Seconds , And Q = 0.25 
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Figure D.13 Near-Minimum Time Tracking Controller Joint Position Time 
History With Gp=1 , Gy=5, Maneuver Time Of 5 Seconds , And @ = 0.1 
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Figure D.14 Near-Minimum Time Tracking Controller Joint Velocity Time 
History With Gp=1 , Gy=5, Maneuver Time Of 5 Seconds , And & = 0.1 
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Figure D.15 Near-Minimum Time Tracking Controller Torque Time History 
With Gp=1 , Gv=5, Maneuver Time Of 5 Seconds , And Q = 0.1 
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Figure D.16 Near-Minimum Time Tracking Controller Wheel Speed Time 
History With Gp=1 , Gy=5, Maneuver Time Of 5 Seconds , And a = 0.1 
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